﻿//#include "stdafx.h"
#include <iostream>
#include <string>
#include <stdio.h>
//
#include "opencv2/opencv.hpp"


using namespace std;
using namespace cv;

const unsigned int cameraID = 0;

int main() {
	//define chess-board variables
	int iNumBoards = 10; //how many successful boards to collect
	int iNumCornersHorizontal = 8;//7
	int iNumCornersVertical = 6;//5
	//get input of chess-board info from users
	double disConersHori=18.75;
	double disConersVer=18.75;

	//setup more variables
	int iNumSquares = iNumCornersHorizontal * iNumCornersVertical;
	Size sBoardSize = Size(iNumCornersHorizontal, iNumCornersVertical);

	//setup video capture
	VideoCapture capture = VideoCapture(cameraID);
	if(!capture.isOpened()) {
		printf("camera %d open failed \n", cameraID);
		return -1;
	}

	/*
	 * important: setup object points and image points
	 */
	vector< vector<Point2f> > imagePoints;
	vector< vector<Point3f> > objectPoints;
	//setup corners
	vector<Point2f> corners;
	vector<Point3f> obj;

	int successes = 0;

	//setup image
	Mat image;
	Mat gray_image;
	capture >> image;

	//setup view win
	namedWindow("win1",1);
	namedWindow("win2", 1);
	
	//setup init obj points
	for(int j = 0; j < iNumSquares; j++){
		obj.push_back(Point3f(j/iNumCornersHorizontal*disConersHori,
				j%iNumCornersHorizontal*disConersVer,
				0.0f));
		//print points
		
		printf("Object[%d]->Point3f(%d, %d, 0)\n",
				j,
				j/iNumCornersHorizontal,
				j%iNumCornersHorizontal);
	}

	//main loop - checking chessboards
	while(successes < iNumBoards){
		capture >> image;
		cvtColor(image, gray_image, CV_BGR2GRAY, 0);

		bool found = findChessboardCorners(image,
				sBoardSize,
				corners,
				CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
		if(found){
			cornerSubPix(gray_image,
					corners,
					Size(11, 11),
					Size(-1, -1),
					TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1)
					);
			drawChessboardCorners(image, sBoardSize, corners, found);
		}
		
		//keyboard event listener
		int key = waitKey(10);
	
		if(key == 32 && found){ //32 is the spacebar key
			bitwise_not(image, image); //to blink
			
			successes ++;
			//
			imagePoints.push_back(corners);
			objectPoints.push_back(obj);
			printf("Got Chessboard Snap %d/%d \n",
					successes,
					iNumBoards);
			//
			if(successes >= iNumBoards){
				printf("Captured ALL successful snaps, exiting... \n");
				break;
			}
			
		}
		
		//show img
		imshow("win1", image);
		imshow("win2", gray_image);
	}
	
	//setup camera matrix and distcoeffic
	Mat intrinsic = Mat(3, 3, CV_32FC1);
	Mat distCoeffs;
	vector<Mat> rvecs;
	vector<Mat> tvecs;
	
	//
	intrinsic.ptr<float>(0)[0] = 1;
	intrinsic.ptr<float>(1)[1] = 1;

	//finds intrinsic and extrinsic camera parameters
	printf("starting calibrating camera... \n");
	calibrateCamera(objectPoints,
			imagePoints,
			image.size(),
			intrinsic,
			distCoeffs,
			rvecs,
			tvecs,
			0,
			TermCriteria( TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
			
		//save the params into file
		printf("saving camera matrix file \n");
		FileStorage camMatrix("intrinsic.yaml", FileStorage::WRITE);
		camMatrix << "imageContent" << intrinsic;
		camMatrix.release();
		printf("saving coefficicnet params \n");
		FileStorage coeffic("coeffic.yaml", FileStorage::WRITE);
		coeffic << "imageContent" << distCoeffs;
		
		//setup img to show undist image
		Mat imageUndist;
		namedWindow("undist", 1);
		
		
		double a00=intrinsic.at<double>(0,0);
		double a01=intrinsic.at<double>(0,1);
		double a02=intrinsic.at<double>(0,2);
		double a10=intrinsic.at<double>(1,0);
		double a11=intrinsic.at<double>(1,1);
		double a12=intrinsic.at<double>(1,2);
		double a20=intrinsic.at<double>(2,0);
		double a21=intrinsic.at<double>(2,1);
		double a22=intrinsic.at<double>(2,2);
	    
		printf("a00=%f\n",a00);
		printf("a01=%f\n",a01);
		printf("a02=%f\n",a02);
		printf("a10=%f\n",a10);
		printf("a11=%f\n",a11);
		printf("a12=%f\n",a12);
		printf("a20=%f\n",a20);
		printf("a21=%f\n",a21);
		printf("a22=%f\n",a22);

		//double aaaa=distCoeffs.at<double>(0,0);
		//printf("da00=%f\n",aaaa);
		double da00=distCoeffs.at<double>(0,0);
		double da01=distCoeffs.at<double>(0,1);
		double da02=distCoeffs.at<double>(0,2);
		double da03=distCoeffs.at<double>(0,3);
		double da04=distCoeffs.at<double>(0,4);
		
		printf("da00=%f\n",da00);
		printf("da01=%f\n",da01);
		printf("da02=%f\n",da02);
		printf("da10=%f\n",da03);
		printf("da11=%f\n",da04);
		
				
	//release resource
	capture.release();
	return 0;
}